#include "rootNode.h"
#include "mat.h"
#include <matrix.h>

#include <stdio.h>
#include <iostream>

#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>
//#include <opencv2/core/core_c.h>

#include <omp.h>

#define LHI_SCAN_RANGE


bool rootNode::sequentialDetection(const char* file_name){
	char mat_file_name[500];
	char template_name[500];

	loadImageToModel(file_name);
/*
#if 0
	inferenceSW(file_name,false);
	sprintf(&mat_file_name[0],"%s_hypoList_sk.mat",file_name);
	saveHypoList(const_cast<const char*>(&mat_file_name[0]));
#else
	sprintf(&mat_file_name[0],"%s_hypoList_sk.mat",file_name);
	readHypoList(const_cast<const char*>(&mat_file_name[0]));
#endif
*/
        sprintf(&mat_file_name[0],"%s_hypoList_sk.mat",file_name);

	if( !readHypoList(const_cast<const char*>(&mat_file_name[0]))){
		inferenceSW(file_name,false);
		saveHypoList(const_cast<const char*>(&mat_file_name[0]));
	}
	hypoBBList.sort();
	int length0=(int)hypoBBList.size();
	printf("List length before color: %d\n",length0);
	hypoBBList.resize(5e3);
	printf("reweighting...\n");
	/*use color to reweight the important hypothesis*/
	for(std::list<BB_s>::iterator itr = hypoBBList.begin();
		itr!=hypoBBList.end(); itr++){
			int arg_max_pos = itr->position;
			int dy = floor((float)arg_max_pos/image.sx);
			int dx = floor((float)arg_max_pos - image.sx*dy);
			/*find projected points*/
			view_s view;
			this->refitP(itr->pose[0],itr->pose[1],itr->pose[2],itr->pose[3],dx,dy,image.sx, image.sy,view);
			rootParticle_s particle; 
			/*Get arg-max structure*/
			this->test(view.P,view.cam_dir,particle);
			//printf("score from %f to %f\n",itr->score,particle.score);
			itr->score = particle.score;
			itr->p_particle=NULL;
	}

	printf("finished reweighting.\n");
#if 0
	this->nonMaxSuppression(0,30,0.8f);
	int hypo_id=0;
	printf("list length after 1st non-max: %d\n",(int)hypoBBList.size());
	rootParticle_s* particle_list = new rootParticle_s[hypoBBList.size()];

	/*use look around to adjust the thypothesis*/
	printf("looking around....");
	for(std::list<BB_s>::iterator itr = hypoBBList.begin();
		itr!=hypoBBList.end(); ++itr,++hypo_id){
			printf("candidate %d," ,hypo_id);
			int arg_max_pos = itr->position;
			int dy = floor((float)arg_max_pos/image.sx);
			int dx = floor((float)arg_max_pos - image.sx*dy);
			/*find projected points*/
			view_s view;
			this->refitP(itr->pose[0],itr->pose[1],itr->pose[2],itr->pose[3],dx,dy,image.sx, image.sy,view);
			rootParticle_s particle; 
			this->lookAroundV2(view.P,particle_list[hypo_id]);
			printf(" from %f to %f.\n",itr->score,particle_list[hypo_id].score);
			itr->score = particle_list[hypo_id].score;
			itr->p_particle=&(particle_list[hypo_id]);
	}
#else
	BB_s* particle_list=new BB_s[3];// 3 is just an arbitrary number
#endif
	this->nonMaxSuppression(0,4,0.5f);
	sprintf(&mat_file_name[0],"%s_results.mat",file_name);
	saveHypoList(const_cast<const char*>(&mat_file_name[0]));
	/*retrive the template for reported BBs*/
	int instance_id = 0;
	for(std::list<BB_s>::iterator itr = this->hypoBBList.begin();
		itr!=hypoBBList.end(); ++itr){
			sprintf(template_name,"%s_instance_%02d.bmp",file_name,instance_id++);
			if (itr->p_particle!=NULL){
			this->argMax(*(itr->p_particle),const_cast<const char*>(&template_name[0]));
			}else{
				 int arg_max_pos = itr->position;
        	                int dy = floor((float)arg_max_pos/image.sx);
                	        int dx = floor((float)arg_max_pos - image.sx*dy);
                        	/*find projected points*/
                        	view_s view;
                        	this->refitP(itr->pose[0],itr->pose[1],itr->pose[2],itr->pose[3],dx,dy,image.sx, image.sy,view);
                        	rootParticle_s particle;
                        	/*Get arg-max structure*/
                        	this->test(view.P,view.cam_dir,particle);
				this->argMax(particle,const_cast<const char*>(&template_name[0]));
			}
	}
	/*save to a matlab file*/
	delete[] particle_list;
	image.release();
	return true;
}



bool rootNode::detection(const char* file_name){
	char mat_file_name[500];
	char template_name[500];

	loadImageToModel(file_name);
	inferenceSW(file_name,true);

	sprintf(&mat_file_name[0],"%s_hypoList.mat",file_name);
	saveHypoList(const_cast<const char*>(&mat_file_name[0]));

	this->nonMaxSuppression(0,4,0.5f);
	sprintf(&mat_file_name[0],"%s_results.mat",file_name);
	saveHypoList(const_cast<const char*>(&mat_file_name[0]));

	/*retrive the template for reported BBs*/
	int instance_id = 0;
	for(std::list<BB_s>::iterator itr = this->hypoBBList.begin();
		itr!=hypoBBList.end(); ++itr){
			int arg_max_pos = itr->position;
			int dy = floor((float)arg_max_pos/image.sx);
			int dx = floor((float)arg_max_pos - image.sx*dy);
			/*find projected points*/
			view_s view;
			this->refitP(itr->pose[0],itr->pose[1],itr->pose[2],itr->pose[3],dx,dy,image.sx, image.sy,view);
			rootParticle_s particle; 
			/*Get arg-max structure*/
			this->test(view.P,view.cam_dir,particle);
			sprintf(template_name,"%s_instance_%02d.bmp",file_name,instance_id++);
			this->argMax(particle,const_cast<const char*>(&template_name[0]));
	}
	/*save to a matlab file*/


	image.release();
	return true;
}


bool rootNode::inferenceSW(const char *file_name, bool use_color){

	/*debug, show the raised proposals*/
	visualizer  window("showP");
	window.setBackground(image.sy,image.sx,image.img_r,image.img_g,image.img_b);

	/*call function to raise hypothesis*/
	this->hypoBBList.clear();

	/*set up search grid*/
	float pan,tilt,roll;
	float pan_step,tilt_step,roll_step;
	pan_step= 45;
	tilt_step = 10;
	roll_step = 10;

	float best_pan,best_tilt,best_roll;
	float best_score,best_dist;
	int best_dx,best_dy;
	best_score = NEGMAX;

	int map_size = image.sx * image.sy;
	float *best_score_map = new float[map_size];
	for(int i = 0; i<map_size; ++i) best_score_map[i]=NEGMAX;

	float *best_pan_map = new float[map_size];
	float *best_tilt_map = new float[map_size];
	float *best_roll_map = new float[map_size];
	float *best_dist_map = new float[map_size];


#ifdef LHI_SCAN_RANGE
	printf("LHI search grid\n");
	omp_set_num_threads(8);
	for(pan = 0; pan<360; pan +=15){
		for(tilt = 5; tilt<90; tilt +=5){
			roll = 0;
		#pragma omp parallel for
			for(int int_dist= 0; int_dist<=7; ++int_dist){
				float dist = 15.0f/(pow(1.10,int_dist));

#else
	printf("regular search grid\n");
	omp_set_num_threads(9);
	int panList[12] = {0,45,60,90,120,135,180,225,240,270,300,315};
	for( int i_pan =0; i_pan<12; ++i_pan) { 
				pan = panList[i_pan];
	for(tilt = 5; tilt<30; tilt+=tilt_step){
	roll = 0;
				/*here openmp kicks in*/
	#pragma omp parallel for
			for(int int_dist= 0; int_dist<=8; ++int_dist){
				float dist = 20.0f/(pow(1.15,int_dist));
#endif


				printf("pan: %5f, tilt %5f, roll %5f, dist %5f\n",pan,tilt,roll,dist);
				view_s view;
				rootParticle_s root_particle;
#pragma omp critical
				{
					raiseViewPoint(pan,tilt,roll,dist,view);
					window.drawP(view.P);
					window.show(5);
					window.cleanImage();
				}

				float *curr_score_map=SWTest(view,use_color);
#pragma omp critical 
				{
					this->cropHypothesisBB(curr_score_map,view,1e-6,100);
				}/*end omp critical*/
				delete[] curr_score_map;
			}/*dist*/
			//			}/*roll*/
		}/*tilt*/
	}/*pan*/

	return true;
}

void rootNode::saveHypoList(const char* mat_file_name){
	MATFile *p_file = matOpen(mat_file_name,"w");
	mxArray *p_tmp_mat = mxCreateNumericMatrix(hypoBBList.size(),10,mxSINGLE_CLASS,mxREAL);
	float *p_array = (float*)mxGetPr(p_tmp_mat);
	int item=0;int total = hypoBBList.size();
	for(std::list<BB_s>::iterator itr = this->hypoBBList.begin();
		itr!=hypoBBList.end(); ++itr){
			p_array[0*total+item]=itr->score;
			p_array[1*total+item]=itr->top;
			p_array[2*total+item]=itr->bottom;
			p_array[3*total+item]=itr->left;
			p_array[4*total+item]=itr->right;
			p_array[5*total+item]=itr->position;
			p_array[6*total+item]=itr->pose[0];
			p_array[7*total+item]=itr->pose[1];
			p_array[8*total+item]=itr->pose[2];
			p_array[9*total+item]=itr->pose[3];
			++item;
	}
	matPutVariable(p_file,"result",p_tmp_mat);
	mxDestroyArray(p_tmp_mat);
	matClose(p_file);
}


bool rootNode::readHypoList(const char* mat_file_name){
	hypoBBList.clear();
	
	MATFile *p_file = matOpen(mat_file_name,"r");
	if(p_file==NULL){
		printf("error, can not open file %s\n, will return.", mat_file_name);
		return false;
	}
	mxArray *p_tmp_mat = matGetVariable(p_file,"result"); 
	if(p_tmp_mat==NULL){
		printf("error, no variable named 'result', will return\n");
		matClose(p_file);
		return false;
	}
	int numElem = mxGetM(p_tmp_mat);
	int numCol = mxGetN(p_tmp_mat);
	assert(numCol==10);
	printf(" %d hypothesises to read, %d colum for each hypothesis\n",numElem,numCol);

	float *p_array = (float*)mxGetPr(p_tmp_mat);
	int total = numElem;
	for(int item=0;item<total; ++item){
			BB_s tmpBB;
			tmpBB.score=p_array[0*total+item];
			tmpBB.top=p_array[1*total+item];
			tmpBB.bottom=p_array[2*total+item];
			tmpBB.left=p_array[3*total+item];
			tmpBB.right=p_array[4*total+item];
			tmpBB.position=p_array[5*total+item];
			tmpBB.pose[0]=p_array[6*total+item];
			tmpBB.pose[1]=p_array[7*total+item];
			tmpBB.pose[2]=p_array[8*total+item];
			tmpBB.pose[3]=p_array[9*total+item];

			hypoBBList.push_back(tmpBB);
	}
	mxDestroyArray(p_tmp_mat);
	matClose(p_file);
	return(true);
}


void rootNode::raiseViewPoint(float pan, float tilt, float roll, float dist, view_s &view){
	pan = pan/180.0f*PI; 
	tilt = tilt/180.0f*PI;/*becaue the tilt angle by our manner is clockwise*/
	roll = roll/180.0f*PI;
	dist = dist *1000; /*conversion from meter to milimeter*/

	cv::Mat basis(3,1,CV_64FC1); 
	cv::Mat R_basis;
	cv::Mat R_perm(3,3,CV_64FC1);
	R_perm.at<double>(0,0) = 0;R_perm.at<double>(0,1) = 1;R_perm.at<double>(0,2) = 0;
	R_perm.at<double>(1,0) = 0;R_perm.at<double>(1,1) = 0;R_perm.at<double>(1,2) = -1;	
	R_perm.at<double>(2,0) = -1;R_perm.at<double>(2,1) = 0;R_perm.at<double>(2,2) = 0;


	basis.at<double>(0,0)=0;
	basis.at<double>(1,0)=pan;
	basis.at<double>(2,0)=0;
	cv::Rodrigues(basis,R_basis);
	cv::Mat3d Rmat= R_basis*R_perm;

	basis.at<double>(0,0)=tilt; basis.at<double>(1,0)=0.0;
	cv::Rodrigues(basis,R_basis);
	Rmat.copyTo(R_perm);
	Rmat = R_basis*R_perm;

	basis.at<double>(0,0)=0; 
	basis.at<double>(2,0)=roll;
	cv::Rodrigues(basis,R_basis);
	Rmat.copyTo(R_perm);
	Rmat = R_basis*R_perm;
	view.cam_dir[0]=Rmat.at<double>(2,0);
	view.cam_dir[1]=Rmat.at<double>(2,1);
	view.cam_dir[2]=Rmat.at<double>(2,2);

	/*this is actually -T, where +T is camera position in world coordinate*/
	cv::Mat Tvec(3,1,CV_64FC1);
	Tvec.at<double>(0,0)=-dist*cos(tilt)*cos(pan);
	Tvec.at<double>(1,0)= -dist*cos(tilt)*sin(pan);
	Tvec.at<double>(2,0)=-dist*sin(tilt);
	view.tx =Tvec.at<double>(0,0); view.ty =Tvec.at<double>(1,0); view.tz = Tvec.at<double>(2,0);



	float f = 600;
	cv::Mat Kmat(3,3,CV_64FC1);
	Kmat.at<double>(0,0) = f;Kmat.at<double>(0,1) = 0;Kmat.at<double>(0,2) = 0;
	Kmat.at<double>(1,0) = 0;Kmat.at<double>(1,1) = f;Kmat.at<double>(1,2) = 0;	
	Kmat.at<double>(2,0) = 0;Kmat.at<double>(2,1) = 0;Kmat.at<double>(2,2) = 1;

	Rmat.copyTo(R_perm);
	Rmat = Kmat * R_perm;
	cv::Mat T_perm(3,1,CV_64FC1);
	Tvec.copyTo(T_perm);
	Tvec = Rmat * T_perm;
	for(int i = 0; i<3; ++i){
		for(int j = 0; j<3; ++j){
			view.P[i][j]=Rmat.at<double>(i,j);
			//			printf("%f,", view.P[i][j]);
		}
		view.P[i][3] =Tvec.at<double>(i,0);
		//		printf("%f \n", view.P[i][3]);
	}

	view.pan = pan; view.tilt =tilt;view.roll = roll;view.distance  = dist;

	/*Then I need to find the deformation range*/
	/*For convenience, only min_ and max_ xy are denoted.*/
	/*User should deduct the range when image size information is available*/

	estimateBB(view);
}



float* rootNode::SWTest(view_s view, bool use_color){
	int sx = image.sx;
	int sy = image.sy;

	float *total_score_map = new float[sx*sy];

	for(int i_pix = 0; i_pix<sx*sy; ++i_pix)
		total_score_map[i_pix] = 0.0f;
	for( int i_node = 0; i_node<this->vol_OR_list.size(); ++ i_node){
		/*call child AND_OR nodes' SWTestFunction*/
		float *tmp_score_map = vol_OR_list[i_node].SWTest(view,use_color);
		/*Add returned score map together*/
		for(int i_pix = 0; i_pix<sx*sy; ++i_pix)
			total_score_map[i_pix]+=tmp_score_map[i_pix];
		/*release memory generated from child nodes*/
		delete[] tmp_score_map;
	}

#ifdef	AREA_NORMALIZE
	float area = (view.max_x-view.min_x)*(view.max_y-view.min_y);
	float area_coef = 1.0f/(area);
#endif

	for( int i_pix = 0; i_pix<sx*sy; ++i_pix){
		if(fabs(total_score_map[i_pix])>1e6){
			total_score_map[i_pix]=0.0f;
		}
#ifdef	AREA_NORMALIZE
		else{
			total_score_map[i_pix]*=area_coef;
		}
#endif
	}



	return total_score_map;
}



/*this is meant to mine hard negative examples, which are assumed to be useful in reweihting*/
/*
compared with inferenceSW function, this function searches through a coaser grid, plus, image data 
is assumed to be managed outside this function
After silding window, it iterates to find best 100 negative examples, and return them by the form of
particles;
*/
bool rootNode::coarseSearch(std::deque<rootParticle_s> &hard_negative){

	printf("Deprecated function, should not be called again!\n");
	/*debug, show the raised proposals*/
	visualizer  window("coarseSearch");
	window.setBackground(image.sy,image.sx,image.img_r,image.img_g,image.img_b);

	/*read ground truth view from training image name*/
	char trash1[500],trash2[500];
	int label_pan,label_pan1,label_pan2;
	//scanf(image.mat_file_name.c_str(),"%s_car_A%d_%s",trash1,label_pan,trash2);
	char* p_num= strrchr(const_cast<char*>(image.mat_file_name.c_str()),'A');
	if(p_num==NULL){
		label_pan = 1440; /*just a large enough value*/
	}else{
		label_pan = atoi(++p_num)-1;
	}

	int pan_tabel[]={90,45,0,315,270,225,180,135};
	int pan_tabel2[]={270,225,180,135,90,45,0,315};
	label_pan1 = pan_tabel[label_pan];
	label_pan2 = pan_tabel2[label_pan];


	/*call function to raise hypothesis*/
	float pan,tilt,roll;
	float pan_step,tilt_step,roll_step;
	pan_step= 15;
	tilt_step = 10;
	roll_step = 10;

	float best_pan,best_tilt,best_roll;
	float best_score,best_dist;
	int best_dx,best_dy;
	best_score = NEGMAX;

	int map_size = image.sx * image.sy;

	for(pan = 0; pan<360; pan +=pan_step){
		if( abs(pan-label_pan1)<30 || abs(pan-label_pan1)>330) continue;
		//if (pan ==label_pan1) continue;
		if (pan ==label_pan2) continue;
		for(tilt = 5; tilt<30; tilt+=tilt_step){
			//for(roll = -10; roll<=10; roll+=roll_step){
			roll = 0;{
				/*here openmp kicks in*/
#pragma omp parallel for
				for(int i_dist = 0; i_dist<12; i_dist+=2){
					float dist = 200.0f/pow(1.15f,(float)i_dist)/10.0f;
					printf("pan: %5f, tilt %5f, roll %5f, dist %5f\n",pan,tilt,roll,dist);
					view_s view;
					rootParticle_s root_particle;
#pragma omp critical
					{
						raiseViewPoint(pan,tilt,roll,dist,view);
						window.drawP(view.P);
						window.show(5);
						window.cleanImage();
					}
					printf("NOT using color for coarse search!\b");
					float *curr_score_map=SWTest(view,false);
#pragma omp critical 
					{

						int i_ptl = 0; 
						while(i_ptl <20){
							float max_score = NEGMAX;
							int arg_max_pos=0;
							for(int i =0; i<map_size; ++i){
								if(max_score<=curr_score_map[i]){
									max_score = curr_score_map[i]; 
									arg_max_pos = i;
								}
							}
							if(max_score <=0) break;
							/*decide dx, dy, dx top-down, starts from 0*/
							int dy = floor((float)arg_max_pos/image.sx);
							int dx = floor((float)arg_max_pos - image.sx*dy);
							/*find projected points*/
							view_s view;
							printf("current candidate:\n pan:%f, tilt:%f, roll:%f, dist:%f\n, dx:%d, dy:%d\n",
								pan,tilt,roll,dist,dx,dy);
							printf("score:%f\n",max_score);
							this->refitP(pan,tilt,roll,dist,dx,dy,image.sx, image.sy,view);
							rootParticle_s particle; 
							/*Get arg-max structure*/
							this->test(view.P,view.cam_dir,particle);
							this->argMax(particle);
							hard_negative.push_back(particle);

							/*inhibite neighborhood*/
							for(int y = std::max(1,dy-4); y<std::min(image.sy,dy+4);++y){
								for(int x  = std::max(1,dx-4); x<std::min(image.sx,dx+4); ++x){ 
									int here = x-1 +(y-1)*image.sx;
									curr_score_map[here]=NEGMAX;
								}
							}
							++i_ptl;
						}/*while*/

					}/*end omp critical*/
					delete[] curr_score_map;
					//printf("Current best:\n");
					//printf("score: %5f, pan: %5f, tilt %5f, roll %5f, dist %5f\n",best_score,best_pan,best_tilt,best_roll,best_dist);
				}/*dist*/
			}/*roll*/
		}/*tilt*/
	}/*pan*/

	return true;
}


bool rootNode::refitP(float pan,float tilt, float roll, float dist, int dx, int dy, int sx, int sy, view_s &view){
	this->raiseViewPoint(pan,tilt,roll,dist,view);
	std::deque<pt_pair_s> pts;

	for(int ipt = 0; ipt<16; ++ipt){
		float u = view.P[0][0]*X[ipt][0] + view.P[0][1]*X[ipt][1] + view.P[0][2]*X[ipt][2] +view.P[0][3];
		float v = view.P[1][0]*X[ipt][0] + view.P[1][1]*X[ipt][1] + view.P[1][2]*X[ipt][2] +view.P[1][3];
		float w = view.P[2][0]*X[ipt][0] + view.P[2][1]*X[ipt][1] + view.P[2][2]*X[ipt][2] +view.P[2][3];

		u = u/w; v= v/w;
		u = u + dy - sy/2.0f;
		v = v + dx - sx/2.0f;
		pt_pair_s pt_pair;
		pt_pair[0]=X[ipt][0]; pt_pair[1]=X[ipt][1]; pt_pair[2]=X[ipt][2];
		pt_pair[3]=u; pt_pair[4]=v;
		pts.push_back(pt_pair);
	}



	float error;
	float dummy_cam_dir[3];
	projector::estimatePerspectiveTransform(pts,view.P,dummy_cam_dir,error);

	estimateBB(view);

	return true;
}



void rootNode::estimateBB(view_s &view){
	// project the data points according to P
	float min_x,min_y,max_x,max_y;
	min_x = -NEGMAX;
	min_y = -NEGMAX;
	max_x = NEGMAX;
	max_y = NEGMAX;
	for(int i = 0; i<28; ++i){
		x[i][0] = view.P[0][0]*X[i][0]+view.P[0][1]*X[i][1]+view.P[0][2]*X[i][2]+view.P[0][3];
		x[i][1] = view.P[1][0]*X[i][0]+view.P[1][1]*X[i][1]+view.P[1][2]*X[i][2]+view.P[1][3];
		x[i][2] = view.P[2][0]*X[i][0]+view.P[2][1]*X[i][1]+view.P[2][2]*X[i][2]+view.P[2][3];

		x[i][0] /=x[i][2];	x[i][1] /=x[i][2];

		min_x= std::min(x[i][0],min_x);
		min_y = std::min(x[i][1],min_y);
		max_x = std::max(x[i][0],max_x);
		max_y = std::max(x[i][1],max_y);
	}
	view.min_x = min_x;
	view.min_y = min_y;
	view.max_x = max_x;
	view.max_y = max_y;
}

void rootNode::cropHypothesisBB(float *score_map, const view_s &view, float threshold, int num){

	int num_bb = 0;

	int position,max_position; 
	int cx = image.sx/2;
	int cy = image.sy/2;

	while(num_bb<=num){
		float max_score = NEGMAX;
		for(int y = 0; y<image.sy; ++y){
			for(int x = 0; x<image.sx; ++x){
				position = y * image.sx + x;
				if(max_score<score_map[position]){
					max_score = score_map[position];
					max_position = position;
				}
			}
		}
		if(max_score<threshold) return;

		/*instantiate the hypothesis*/
		int inst_y = floor((float)max_position/image.sx);
		int inst_x = max_position-image.sx*inst_y;
		BB_s bb_inst;
		bb_inst.top = view.min_y + inst_x;
		bb_inst.bottom = view.max_y + inst_x;
		bb_inst.left = view.min_x +  inst_y;
		bb_inst.right = view.max_x + inst_y;
		bb_inst.position = max_position;
		bb_inst.score = max_score;
		bb_inst.pose[0]=view.pan/PI*180;
		bb_inst.pose[1]=view.tilt/PI*180;
		bb_inst.pose[2]=view.roll/PI*180;
		bb_inst.pose[3]=view.distance/1000;
		hypoBBList.push_back(bb_inst);
		score_map[max_position]=NEGMAX;
		/*inhibition*/
		//int radius = 5; 
		//for(int iy = std::max(0,inst_y-5); iy<std::min(image.sy,inst_y+5); ++iy){
		//for( int ix = std::max(0,inst_x-5); ix<std::min(image.sx,inst_x+5); ++ix){
		//		score_map[ix + iy *image.sx]=NEGMAX;
		//	}
		//}

		++num_bb;
	}/* while */

}

float rootNode::BBOverlap(std::list<BB_s>::iterator itr1,std::list<BB_s>::iterator itr2){
	/* from line 49 of rectint.m
	* out = (max(0, min(rightA, rightB) - max(leftA, leftB))) .* ...
	* (max(0, min(topA, topB) - max(bottomA, bottomB)));
	*/

	float intersect,overlap;
	intersect = std::max(0,std::min(itr1->right,itr2->right)-std::max(itr1->left,itr2->left));
	intersect *= std::max(0,std::min(itr1->bottom,itr2->bottom)-std::max(itr1->top,itr2->top));

	float union_area = (itr1->bottom-itr1->top)*(itr1->right-itr1->left);
	union_area +=(itr2->bottom-itr2->top)*(itr2->right-itr2->left);
	union_area -=intersect;

	overlap = intersect/union_area;

	return overlap;
}

void rootNode::nonMaxSuppression(float threshold, int num, float max_overlap){
	/*sort the list*/
	hypoBBList.sort();
	/*do inhibition, suppose hypoBBlist is sorted in dscent order*/
	std::list<BB_s>::iterator itr1, itr2; 
	int num_item = 0; 
	itr1 = hypoBBList.begin(); 

	if(hypoBBList.empty()){
		printf("hypoBBList is empty!!!\n");
		return;
	}
	while(num_item<=num&&itr1!=hypoBBList.end()){
		itr2 = ++itr1;
		--itr1;
		while(itr2!=hypoBBList.end()){
			if( BBOverlap(itr1,itr2)>max_overlap){
				itr2=hypoBBList.erase(itr2);
			}else{
				++itr2;
			}
		}
		++itr1;
		++num_item;
	}
	/*delete the remaining hypothsis*/
	if(itr1!=hypoBBList.end()){
		++itr1;
		hypoBBList.erase(itr1,hypoBBList.end());
	}
}




bool rootNode::cropHardNegExample(const char *file_name,std::deque<rootParticle_s> &hard_negative ){
	char mat_file_name[500];
	char template_name[500];

	hard_negative.clear();

	/*read ground truth*/
	sprintf(&mat_file_name[0],"%s.view.mat",file_name);
	MATFile *p_File = matOpen(&mat_file_name[0],"r");
	mxArray *p_BB = matGetVariable(p_File,"gt_bd");
	double *BB = (double*)mxGetData(p_BB);
	std::list<BB_s> GTBBList;
	BB_s GTBB; 
	GTBB.left=BB[0];
	GTBB.right=BB[1];
	GTBB.top = BB[2];
	GTBB.bottom=BB[3];

	mxDestroyArray(p_BB);
	matClose(p_File);

	GTBBList.push_back(GTBB);

	/*call inference function*/
	loadImageToModel(file_name);
	printf("not using color!!\n");
	inferenceSW(file_name,false);

	sprintf(&mat_file_name[0],"%s_hypoList.mat",file_name);
	saveHypoList(const_cast<const char*>(&mat_file_name[0]));

	this->nonMaxSuppression(0,1000,0.7f);
	/*further prune correct detection to get negative examples*/
	int instance_id = 0;
	for(std::list<BB_s>::iterator itr = this->hypoBBList.begin();
		itr!=hypoBBList.end(); ++itr){
			if(BBOverlap(itr,GTBBList.begin())>0.5){
				continue;
			}

			int arg_max_pos = itr->position;
			int dy = floor((float)arg_max_pos/image.sx);
			int dx = floor((float)arg_max_pos - image.sx*dy);
			/*find projected points*/
			view_s view;
			this->refitP(itr->pose[0],itr->pose[1],itr->pose[2],itr->pose[3],dx,dy,image.sx, image.sy,view);
			rootParticle_s particle; 
			/*Get arg-max structure*/
			this->test(view.P,view.cam_dir,particle);
			sprintf(&template_name[0],"%s_neg_%03d.png",file_name,instance_id);
			//this->argMax(particle,const_cast<const char*>(&template_name[0]));
			this->argMax(particle);
			hard_negative.push_back(particle);
			++instance_id;
	}

	/*clear image data*/
	image.release();
	return true;
}





bool rootNode::randomCollectNegExample(const char *file_name,int num_particle, std::deque<rootParticle_s> &negative ){
	char mat_file_name[500];
	char template_name[500];

	negative.clear();

	/*read ground truth*/
	sprintf(&mat_file_name[0],"%s.view.mat",file_name);
	MATFile *p_File = matOpen(&mat_file_name[0],"r");
	mxArray *p_BB = matGetVariable(p_File,"gt_bd");
	double *BB = (double*)mxGetData(p_BB);
	std::list<BB_s> GTBBList;
	BB_s GTBB; 
	GTBB.left=BB[0];
	GTBB.right=BB[1];
	GTBB.top = BB[2];
	GTBB.bottom=BB[3];

	mxDestroyArray(p_BB);
	matClose(p_File);

	GTBBList.push_back(GTBB);

	/*read image data*/
	loadImageToModel(file_name);
	/*generate random bounding boxes*/
	int panList[24];
	for( int i = 0; i<24; ++i) panList[i]=i*15;
	int tiltList[3]={5,15,25};
	float  distList[16];
	for(int i = 0; i<16; ++i) distList[i]=5+i;

	for( int i_inst = 0; i_inst<num_particle; ){
		int pan = panList[rand()%24];
		int tilt = tiltList[rand()%3];
		float dist = distList[rand()%16];
		int dx = rand()%image.sx - image.sx/2;
		int dy = rand()%image.sy - image.sy/2;
		view_s view;
		refitP(pan,tilt,0,dist,dx,dy,image.sx,image.sx,view);
		BB_s BB;
		BB.top = view.min_y + image.sx/2;
		BB.bottom = view.max_y + image.sx/2;
		BB.left = view.min_x + image.sy/2;
		BB.right = view.max_x + image.sy/2;
		/*decide if out of boundary*/
		if(BB.left<10|| BB.right> image.sy-10 || BB.top<10 || BB.bottom>image.sx-10)
			continue;
		/*decide if coincide with image*/
		std::list<BB_s> BBList;
		BBList.push_back(BB);
		if( BBOverlap(BBList.begin(),GTBBList.begin())>0.7 ) 
			continue;
		/*get argMax*/
		//	printf("inst: %d,  pan: %d, tilt: %d, dist: %f, dx: %d, dy: %d\n",i_inst,pan,tilt,dist,dx,dy);
		rootParticle_s particle;
		test(view.P,view.cam_dir,particle);
		sprintf(&template_name[0],"%s_neg_%02d.png",file_name,i_inst);
		argMax(particle);
		//argMax(particle,const_cast<const char*>(&template_name[0]));
		/*push to feature List*/
		negative.push_back(particle);
		/*increment counter*/
		++i_inst;
	}

	/*clear image data*/
	image.release();
	return true;
}
